import sys
import Image

global im

class Node(object):
     def __init__(self,x,y):
          self.x=x
          self.y=y
     def __eq__(self,node):
          return self.x==node.x and self.y==node.y
     def __str__(self):
          return '(%d, %d)'%(self.x,self.y)
     def __repr__(self):
          return self.__str__()
     def __hash__(self):
          return hash(self.x*sys.maxint+self.y)

class astar():
     def __init__(self, start, end, is_end, neighbor_nodes, dist_between, estimated_distance_to_goal):
          self.start = start
          self.end = end
          self.is_end = is_end;
          self.neighbor_nodes = neighbor_nodes
          self.dist_between = dist_between
          self.estimated_distance_to_goal = estimated_distance_to_goal

     def findPath(self):
          closedlist = []                # The set of nodes already evaluated.
          openlist = [self.start]             # The set of tentative nodes to be evaluated.
          g_score = {}
          h_score = {}
          f_score = {}
          g_score[self.start] = 0             # Distance from start along optimal path.
          h_score[self.start] = self.estimated_distance_to_goal(self.start)
          f_score[self.start] = g_score[self.start] + h_score[self.start]
          came_from = {}
          while openlist:
               x = reduce(lambda x,y:f_score[x]>f_score[y] and y or x, openlist)
               if self.is_end(x):
                    return self.path_traced_through(came_from)
               openlist.remove(x)
               closedlist.append(x)
               for y in self.neighbor_nodes(x):
                    if y in closedlist:
                         continue
                    tentative_g_score = g_score[x] + self.dist_between(x,y)
                    tentative_is_better = False
                    if y not in openlist:
                         openlist.append(y)
                         h_score[y] = self.estimated_distance_to_goal(y)
                         tentative_is_better = True
                    elif tentative_g_score < g_score[y]:
                         tentative_is_better = True
                    if tentative_is_better == True:
                         came_from[y] = x
                         g_score[y] = tentative_g_score
                    f_score[y] = g_score[y] + h_score[y] # Estimated total distance from start to goal through y.
          return None
     def path_traced_through(self, came_from):
          path = []
          n=self.end
          path.append(n)
          while n!=self.start:
               n = came_from[n]
               path.append(n)
          return path
               
               
def neighbor(node):
     global im
     for x,y in [(0,1), (1,0), (0,-1), (-1,0)]:
          nn =  Node(node.x+x,node.y+y)
          if nn.x>=0 and nn.x<20 and nn.y>=0 and nn.y<20 \
                 and im[nn.x, nn.y]!=(0,0,0):
               yield nn

def dist_between(nodex,nodey):
     return abs(nodex.x-nodey.x) + abs(nodex.y-nodey.y)

def estimated_distance_to_goal(node):
     return dist_between(node,Node(18,19))

def init():
     global im,img
     f = open('test.csv')
     w=len(f.readline().split(','))
     h=0
     
     f.seek(0)
     for line in f:
          if len(line.split(','))>0:
               h+=1
     img0 = Image.new('RGB',(w,h),(255,255,255))
     im0 = img0.load()
     f.seek(0)
     for y,line in enumerate(f):
          for x,c in enumerate(line.split(',')):
               if int(c) == 1:
                    im0[x,y] = (0,0,0)
     img0.save('test.png')
     img = Image.new('RGB',(w,h))
     img.paste(img0.copy())
     im = img.load()

def main():
     global im

     init()
     
     is_end=lambda x:x.x==18 and x.y==19
     
     start = Node(1,0)
     end = Node(18,19)
     as = astar(start, end, is_end, neighbor, dist_between, estimated_distance_to_goal)
     r = as.findPath()
     if r:
          for node in r[::-1]:
               print node.x,node.y
          for node in r:
               im[node.x,node.y]=127
          img.save('test_solved.png')
     else:
          print "failed"
          
          

if __name__ == '__main__':
     main()
     
